#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     frontr,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     backr,         tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     frontl,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     backl,         tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    d1,                   tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    cup1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    lg1,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    rg1,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_1,    d2,                   tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    cup2,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    lg2,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    rg2,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void close(){
servo[d2]=0;
servo[cup2]=0;
servo[d1]=255;
servo[cup1]=255;}
void open(){
servo[d2]=255;
servo[cup2]=255;
servo[d1]=0;
servo[cup1]=0;}


void initializeRobot()
{

  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
 servo[d1]=255;
servo[cup1]=255;
servo[d2]=0;
servo[cup2]=0;
 servo[rg1]=128;
      servo[rg2]=128;
         servo[lg1]=255-90;
      servo[lg2]=90;

servoChangeRate[lg1]=4;
servoChangeRate[lg2]=4;
return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////



task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {
	  getJoystickSettings(joystick);


        motor[backr]=joystick.joy1_y1;
    motor[frontr]=joystick.joy1_y1   ;
    motor[backl]=joystick.joy1_y2;
    motor[frontl]=joystick.joy1_y2;





if (joy2Btn(8))
    {
     servo[lg1]=190;
servo[lg2]=65;
    }     if (joy2Btn(6))
    {


      servo[lg1]=55;
servo[lg2]=200;
    }

    if (joy1Btn(6))
    {
      servo[rg1]=0;
      servo[rg2]=255;
    }

     else if (joy1Btn(8))
    {
      servo[rg1]=155;
      servo[rg2]=100;
    }




    if (joy2Btn(4))
    {

      motor[motorB]=100;

  }

else if (joy2Btn(2))
    {

      motor[motorB]=-50;

  }
    else
  {motor[motorB]=0;}
  if (joy2Btn(1))
  {close();}
  else if (joy2btn(3))
  {open();}



}
}
